#include "Drivetrain.h"
#include "../Robotmap.h"
#include "SmartDashboard/SmartDashboard.h"

Drivetrain::Drivetrain() :
	Subsystem("Drivetrain"), leftJag(JAGUAR_LEFT), rightJag(JAGUAR_RIGHT),
			leftEncoder(ENCODER_LEFT_A, ENCODER_LEFT_B, true, Encoder::k4X),
			rightEncoder(ENCODER_RIGHT_A, ENCODER_RIGHT_B, true, Encoder::k4X),
			leftPid(kPLeft, kILeft, kDLeft, &leftEncoder, &leftJag),
			rightPid(kPRight, kIRight, kDRight, &rightEncoder, &rightJag),
			robotDrive(&leftJag, &rightJag) {
	robotDrive.SetSafetyEnabled(false);
	leftEncoder.Reset();
	rightEncoder.Reset();
	leftEncoder.Start();
	rightEncoder.Start();
	leftEncoder.SetDistancePerPulse(.0368);
	rightEncoder.SetDistancePerPulse(.0368);
	distanceDriving = false;
}

float Drivetrain::Scale(float value, float min, float max, float newmin,
		float newmax) {
	float scaled;
	scaled = ((newmax - newmin) / (max - min)) * (value - min) + newmin;
	return scaled;

}
float Drivetrain::DeadZone(float input, float dz) {
	if (fabs(input) < dz) {
		input = 0;
	}
	return input;
}

void Drivetrain::InitDefaultCommand() {
	// Set the default command for a subsystem here.
	//SetDefaultCommand(new MySpecialCommand());
	SetDefaultCommand(new TeleOpCommand());
}

void Drivetrain::CheckDistanceDrive() {
	if (!distanceDriving) {
		return;
	}
	if (fabs(leftPid.GetError()) < kLDistanceThresh
			&& fabs(rightPid.GetError()) < kRDistanceThresh) {
		distanceDriving = false;
		leftEncoder.SetPIDSourceParameter(Encoder::kRate);
		rightEncoder.SetPIDSourceParameter(Encoder::kRate);
	}
}

void Drivetrain::DriveDistance(float dL, float dR) {
	distanceDriving = true;
	leftEncoder.SetPIDSourceParameter(Encoder::kDistance);
	rightEncoder.SetPIDSourceParameter(Encoder::kDistance);
	leftEncoder.Reset();
	rightEncoder.Reset();
	leftPid.SetSetpoint(dL);
	rightPid.SetSetpoint(dR);

}

void Drivetrain::DriveRobot(float X, float Y) {

	leftPid.SetPID(CommandBase::client->P, CommandBase::client->I,
			CommandBase::client->D);
	rightPid.SetPID(CommandBase::client->P, CommandBase::client->I,
			CommandBase::client->D);
	//leftPid.Enable();
	//rightPid.Enable();
	if (distanceDriving) {
		CheckDistanceDrive();
		return;
	}

	/*if (true
	 || DriverStation::GetInstance()->IsAutonomous()) {
	 float kXDeadZone = .1;
	 float kYDeadZone = .1;
	 //float kThrottleDeadZone = .1;

	 X = DeadZone(X, kXDeadZone);
	 Y = DeadZone(Y, kYDeadZone);
	 float left = X + Y;
	 float right = X - Y;
	 if (left > 1) {
	 left = 1;
	 } else if (left < -1) {
	 left = -1;
	 }
	 if (right > 1) {
	 right = 1;
	 } else if (right < -1) {
	 right = -1;
	 }

	 leftPid.SetSetpoint(left * kWheelSpeed);
	 rightPid.SetSetpoint(right * kWheelSpeed);
	 } else {
	 leftPid.SetSetpoint(0);
	 rightPid.SetSetpoint(0);
	 }	*/

	if (true) {
		float kXDeadZone = .1;
		float kYDeadZone = .1;

		X = -DeadZone(X, kXDeadZone);
		Y = -DeadZone(Y, kYDeadZone);
		if (X < 0) {
			X = Scale(X, -1, -kXDeadZone, -1, 0);
		} else {
			X = Scale(X, kXDeadZone, 1, 0, 1);
		}
		if (Y < 0) {
			Y = Scale(Y, -1, -kXDeadZone, -1, 0);
		} else {
			Y = Scale(Y, kXDeadZone, 1, 0, 1);
		}
		robotDrive.ArcadeDrive(-Y, X, true);
	} else {
		robotDrive.ArcadeDrive(0, 0, false);
	}
}
